#include "ros/ros.h"
#include "cyberarm_controller/me_msg.h"

#define MOTOR_NUM 4

// 用于实时获取电机指令(mode = 2)
int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ROS_INFO("property_publisher_node");
    ros::init(argc, argv, "property_publisher_node");
    ros::NodeHandle nh;
    ros::Publisher motor_enable_pub = nh.advertise<cyberarm_controller::me_msg>("motor_enable", 10); 
    ros::Rate rate_delay(1); // 延时1s等待CAN接口初始化之后再进行reply_state配置，否则可能会出现配置不成功的情况
    rate_delay.sleep();
    
    ros::Rate rate(100);
    while (ros::ok())
    {
        for(unsigned int i = 0; i < MOTOR_NUM; i++)
        {
            cyberarm_controller::me_msg i_message;
            i_message.id = i + 1;
            i_message.cmd_mode = 2;  // 电机使能
            motor_enable_pub.publish(i_message);
        }   
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}
